#include "MyTemperature.h"

MyTemperature::MyTemperature(void)
{
    for (size_t i = 0; i < sizeof(CS_PIN); i++)
    {
        rtd[i] = new MAX31865_RTD(MAX31865_RTD::RTD_PT100, CS_PIN[i]);
        /* Configure:
       V_BIAS enabled
       Auto-conversion
       1-shot disabled
       3-wire enabled
       Fault detection:  automatic delay
       Fault status:  auto-clear
       50 Hz filter
       Low threshold:  0x0000
       High threshold:  0x7fff
       正常raw值会在5000-15000之间
     */

        rtd[i]->configure(true, true, false, true, MAX31865_FAULT_DETECTION_NONE, true, true, 1000, 30000);
    }
}
////////////////////////////////////////////////////
//////////////////////
// void MyTemperature::recalulateAndSetRaw0(double t, uint16_t *raw)
// {

//     for (size_t i = 0; i < SENSER_QUANTITY; i++)
//     {

//         int r = raw[i];

//         if (r < 1000 || r > 30000) //超限
//         {
//             Serial.printf("S%d over limit. raw=%u\n", i, r);
//             continue;
//         }

//         if (t < 0) //0度以下不校准
//         {
//             Serial.println("not calibate under 0 C\n");
//             return;
//         }

//         rawKeyPoint[i].raw_0 = calulateRaw0(t, r);
//     }
// }
//////////////////

// void MyTemperature::recalulateAndSetKeyPoint(double t, uint16_t *raw)
// {

//     for (size_t i = 0; i < SENSER_QUANTITY; i++)
//     {

//         int r = raw[i];

//         if (r < 1000 || r > 30000) //超限
//             continue;

//         if (t < 0) //0度以下不校准
//             return;

//         if (t == 0)
//         { //设置0度
//             rawKeyPoint[i].raw_0 = r;
//         }
//         else if (t < 75)
//         { //用0度和t推算R50
//             uint16_t r0 = rawKeyPoint[i].raw_0;
//             double rate = r - r0; //int->double
//             rate = rate / t;
//             rawKeyPoint[i].raw_50 = (uint16_t)(r0 + rate * 50);
//         }
//         else if (t < 125)
//         { //用50推算R100
//             uint16_t r50 = rawKeyPoint[i].raw_50;
//             double rate = r - r50;
//             rate = rate / (t - 50);
//             rawKeyPoint[i].raw_100 = (uint16_t)(r50 + rate * 50);
//         }
//         else if (t < 175)
//         { //调整点150

//             Serial.printf("S%d origin P150=%d ", i, rawKeyPoint[i].raw_150);

//             uint16_t r100 = rawKeyPoint[i].raw_100;
//             double rate = r - r100;
//             rate = rate / (t - 100);
//             rawKeyPoint[i].raw_150 = (uint16_t)(r100 + rate * 50);

//             ////////////////////////////
//             Serial.printf("set to %d\n", rawKeyPoint[i].raw_150);
//             /////////////
//         }
//         else if (t < 225)
//         { //调整r200
//             uint16_t r150 = rawKeyPoint[i].raw_150;
//             double rate = r - r150;
//             rate = rate / (t - 150);
//             uint16_t r200 = (uint16_t)(r150 + rate * 50);
//             rawKeyPoint[i].raw_200 = r200;
//         }
//         else
//         { //调整r250
//             uint16_t r200 = rawKeyPoint[i].raw_200;
//             double rate = r - r200;
//             rate = rate / (t - 200);
//             uint16_t r250 = (uint16_t)(r200 + rate * 50);
//             rawKeyPoint[i].raw_250 = r250;
//         }
//     }
// }

// void MyTemperature::setRawKeyPoint(RawKeyPoint *keyPoint)
// {
//     this->rawKeyPoint = keyPoint;
// }

uint16_t MyTemperature::getRawResistance(uint8_t senserNumber)
{

    if (senserNumber >= SENSER_QUANTITY)
    {
        Serial.print("Warning! wrong senserNumber=");
        Serial.println(senserNumber);
        return 0;
    }

    int status = rtd[senserNumber]->read_all();

    if (0 != status)
    {
        Serial.print("status=");
        Serial.println(status);

        if (status & MAX31865_FAULT_HIGH_THRESHOLD)
        {
            Serial.println("RTD high threshold exceeded");
        }
        else if (status & MAX31865_FAULT_LOW_THRESHOLD)
        {
            Serial.println("RTD low threshold exceeded");
        }
        else if (status & MAX31865_FAULT_REFIN)
        {
            Serial.println("REFIN- > 0.85 x V_BIAS");
        }
        else if (status & MAX31865_FAULT_REFIN_FORCE)
        {
            Serial.println("REFIN- < 0.85 x V_BIAS, FORCE- open");
        }
        else if (status & MAX31865_FAULT_RTDIN_FORCE)
        {
            Serial.println("RTDIN- < 0.85 x V_BIAS, FORCE- open");
        }
        else if (status & MAX31865_FAULT_VOLTAGE)
        {
            Serial.println("Overvoltage/undervoltage fault");
        }
        else
        {
            Serial.println("Unknown fault; check connection");
        }

        Serial.print("raw=");
        Serial.println(rtd[senserNumber]->raw_resistance());

        this->raw[senserNumber] = 0;
    }
    else
    {
        this->raw[senserNumber] = rtd[senserNumber]->raw_resistance();
    }

    return this->raw[senserNumber];
}

double MyTemperature::getTemperature(uint16_t raw_t, int senserNumber)
{

    if (raw_t < RawMin || raw_t > RawMax)
    {
        Serial.printf("raw%d=%u overflow\n", senserNumber,raw_t);
        return -300;
    }

    double a0 = this->calibratePoint[senserNumber].parameter[0];
    double a1 = this->calibratePoint[senserNumber].parameter[1];
    double a2 = this->calibratePoint[senserNumber].parameter[2];

    if ((isnan(a0)) || a0 < RawMin || a0 > RawMax)
    {
        ///////////////////////////
        Serial.printf("p0=%f overflow;senser=%d\n", a0, senserNumber);
        ///////////////
        return -300;
    }

    double c = a0 - raw_t;
    double b = a1;
    double a = a2;

    double a_2 = 2.0 * a;
    double b_sq = b * b;

    double D = b_sq - 2.0 * a_2 * c;

    double temperature_deg_C = (-b + sqrt(D)) / a_2;
    return temperature_deg_C;
}

void MyTemperature::recalulateAndSetCelibrate(double temperature, uint16_t *raw)
{

    for (size_t i = 0; i < SENSER_QUANTITY; i++)
    {

        CalibratePoint cp = calibratePoint[i];

        //检查校准点是否已满
        boolean isFull = true;
        int index = 0;
        for (size_t j = 0; j < CALIBRATE_POINT_MAX; j++)
        {
            uint16_t a = cp.raw[j];
            if (isnan(a) || a < RawMin || a > RawMax)
            {
                isFull = false;
                index = j;
                break;
            }
        }

        if (isFull)
        { //已满，检查temperature 差小于10度的
            boolean hasMin50 = false;
            int idx = 0;

            for (size_t k = 0; k < CALIBRATE_POINT_MAX; k++)
            {
                if (abs(cp.t[k] - temperature) < 10)
                {
                    idx = k;
                    hasMin50 = true;
                    break;
                }
            }

           if (hasMin50)
            { //将index后面的数据前移
                for (size_t j = idx; j < CALIBRATE_POINT_MAX - 1; j++)
                {
                    calibratePoint[i].t[j] = calibratePoint[i].t[j + 1];
                    calibratePoint[i].raw[j] = calibratePoint[i].raw[j + 1];
                }
            }
            else
            { //将0位迁移
                for (size_t j = 0; j < CALIBRATE_POINT_MAX - 1; j++)
                {
                    calibratePoint[i].t[j] = calibratePoint[i].t[j + 1];
                    calibratePoint[i].raw[j] = calibratePoint[i].raw[j + 1];
                }
            }

            //在最后一位插入
            calibratePoint[i].raw[CALIBRATE_POINT_MAX - 1] = raw[i];
            calibratePoint[i].t[CALIBRATE_POINT_MAX - 1] = temperature;
        }
        else
        { //未满，直接添加在index里面
            calibratePoint[i].raw[index] = raw[i];
            calibratePoint[i].t[index] = temperature;
        }

        /////////////////
        ///启动校验计算
        MyFit fit;
        //计算有效数据个数
        int count = CALIBRATE_POINT_MAX;
        if (!isFull)
        { //数据未满
            count = index;
        }

        if (count < FIT_RANK + 1)
        { //校准点至少要3个
            continue;
        }

        fit.setData(calibratePoint[i].t, calibratePoint[i].raw, count);

        double *result = fit.fit();

        for (size_t j = 0; j < FIT_RANK + 1; j++)
        {
            calibratePoint[i].parameter[j] = result[j];
        }
    }
}

// myFit = new MyFit();
// uint16_t raw[] = {8915, 10349, 11487, 7496, 12603, 13649};
// double t[] = {50, 100, 150, 0, 200, 250};
// myFit->setData(t, raw, sizeof(t) / sizeof(double));
// myFit->fit();

// double MyTemperature::getTemperature(uint8_t senserNumber)
// {

//     RawKeyPoint kp = rawKeyPoint[senserNumber];

//     uint16_t raw_0 = this->rawKeyPoint[senserNumber].raw_0;
//     if (raw_0 < 5000 || raw_0 > 10000)
//     { //超出正常值
//         // raw_0 = 7600;
//         return -300;
//     }

//     uint16_t raw_100 = this->rawKeyPoint[senserNumber].raw_100;
//     if (raw_100 < 8000 || raw_100 > 12000)
//     { //超出正常值
//         //raw_100 = 10300;
//         return -300;
//     }

//     uint16_t raw_resistance = this->raw[senserNumber];

//     double rate50 = kp.raw_50 - kp.raw_0;
//     double t = raw_resistance - raw_0;
//     t = t / rate50 * 50;
//     if (t < 50)
//         return t;

//     //超过50度，重新计算
//     if (raw_resistance <= kp.raw_100)
//     {                                               //100度以内
//         double rate50_100 = kp.raw_100 - kp.raw_50; //50到100的斜率
//         t = raw_resistance - kp.raw_50;
//         t = 50 + t / rate50_100 * 50;
//         return t;
//     }

//     if (raw_resistance <= kp.raw_150) //100-150
//     {
//         double rate = kp.raw_150 - kp.raw_100;
//         t = raw_resistance - kp.raw_100;
//         t = 100 + t / rate * 50;
//         return t;
//     }

//     if (raw_resistance <= kp.raw_200) //150-200
//     {
//         double rate = kp.raw_200 - kp.raw_150;
//         t = raw_resistance - kp.raw_150;
//         t = 150 + t / rate * 50;
//         return t;
//     }

//     //200以上
//     double rate = kp.raw_250 - kp.raw_200;
//     t = raw_resistance - kp.raw_200;
//     t = 200 + t / rate * 50;
//     return t;
// }

// RawKeyPoint *MyTemperature::calulateAndSetKeyPointAll()
// {
//     for (size_t i = 0; i < SENSER_QUANTITY; i++)
//     {
//         uint16_t raw_0 = this->rawKeyPoint[i].raw_0;
//         if (raw_0 < 5000 || raw_0 > 10000)
//         { //超出正常值
//             continue;
//         }

//         uint16_t raw_100 = this->rawKeyPoint[i].raw_100;
//         if (raw_100 < 8000 || raw_100 > 12000)
//         { //超出正常值
//             continue;
//         }

//         double rate100 = raw_100 - raw_0; //注意int转double，不要简写
//         rate100 = rate100 / 100;          //0-100斜率

//         double rate50 = rate100 * RATE_0_50; //0-50斜率
//         uint16_t rt50 = raw_0 + rate50 * 50; //计算50度时对应的raw值
//         rawKeyPoint[i].raw_50 = rt50;

//         double rate100_150 = rate100 * RATE_100_150; //100到150的斜率
//         uint16_t rt150 = raw_100 + rate100_150 * 50; //计算150度时对应的raw值
//         rawKeyPoint[i].raw_150 = rt150;

//         double rate150_200 = rate100 * RATE_150_200; //150到200的斜率
//         uint16_t rt200 = rt150 + rate150_200 * 50;   //计算200度时对应的raw值
//         rawKeyPoint[i].raw_200 = rt200;

//         double rate200_250 = rate100 * RATE_200_250; //200-250的斜率
//         uint16_t rt250 = rt200 + rate200_250 * 50;
//         rawKeyPoint[i].raw_250 = rt250;
//     }

//     return rawKeyPoint;
// }

// uint16_t raw_0 = this->rawKeyPoint[senserNumber].raw_0;
// if (raw_0 < 5000 || raw_0 > 10000)
// { //超出正常值
//     // raw_0 = 7600;
//     return -300;
// }

// uint16_t raw_100 = this->rawKeyPoint[senserNumber].raw_100;
// if (raw_100 < 8000 || raw_100 > 12000)
// { //超出正常值
//     //raw_100 = 10300;
//     return -300;
// }

// uint16_t raw_resistance = this->raw[senserNumber];

// double rate100 = raw_100 - raw_0;    //注意int转double，不要简写
// rate100 = rate100 / 100;             //0-100斜率
// double rate50 = rate100 * RATE_0_50; //0-50斜率
// double temperature2 = raw_resistance - raw_0;
// temperature2 = temperature2 / rate50;

// if (temperature2 >= 50) //超过50度，重新计算
// {
//     uint16_t rt50 = raw_0 + rate50 * 50;       //计算50度时对应的raw值
//     double rate50_100 = rate100 * RATE_50_100; //50到100的斜率
//     temperature2 = 50 + (raw_resistance - rt50) / rate50_100;
// }

// if (temperature2 >= 100) //超过100度，重新计算
// {
//     double rate100_150 = rate100 * RATE_100_150; //100到150的斜率
//     temperature2 = 100 + (raw_resistance - raw_100) / rate100_150;
// }

// if (temperature2 >= 150)
// {
//     double rate100_150 = rate100 * RATE_100_150; //100到150的斜率
//     uint16_t rt150 = raw_100 + rate100_150 * 50; //计算150度时对应的raw值
//     double rate150_200 = rate100 * RATE_150_200; //150到200的斜率
//     temperature2 = 150 + (raw_resistance - rt150) / rate150_200;

//     if (temperature2 >= 200)
//     {
//         uint16_t rt200 = rt150 + rate150_200 * 50;   //计算200度时对应的raw值
//         double rate200_250 = rate100 * RATE_200_250; //200-250的斜率
//         temperature2 = 200 + (raw_resistance - rt200) / rate200_250;
//     }
// }
